#include "main.h"
#include "tim.h"
#include "Hardware_Motor.h"
#include "Software_Chassis.h"
#include <stdlib.h>
#include <math.h>

#define WHEEL_SPACING_FB 0.2f // 前后轮轴间距的1/2
#define WHEEL_SPACING_LR 0.2f // 左右轮轴间距的1/2

Motor_s_Typedef LF_Motor;
Motor_s_Typedef RF_Motor;
Motor_s_Typedef LB_Motor;
Motor_s_Typedef RB_Motor;

float Chassis_max(float a, float b, float c, float d)
{
    float max_value = a;
    if (b > max_value) max_value = b;
    if (c > max_value) max_value = c;
    if (d > max_value) max_value = d;
    return max_value;
}

/**
 * @brief 电机初始化
 * 
 */
void Chassis_Init(void)
{
	LF_Motor.PWM1_Reso 		  = 1000;
	LF_Motor.PWM1_UsedTIM 	  = &htim2;
	LF_Motor.PWM1_UsedChannel = TIM_CHANNEL_3;
    LF_Motor.PWM2_Reso        = 1000;
    LF_Motor.PWM2_UsedTIM 	  = &htim2;
	LF_Motor.PWM2_UsedChannel = TIM_CHANNEL_4;

    RF_Motor.PWM1_Reso        = 1000;
    RF_Motor.PWM1_UsedTIM 	  = &htim8;
	RF_Motor.PWM1_UsedChannel = TIM_CHANNEL_1;
    RF_Motor.PWM2_Reso        = 1000;
    RF_Motor.PWM2_UsedTIM 	  = &htim8;
	RF_Motor.PWM2_UsedChannel = TIM_CHANNEL_2;

    LB_Motor.PWM1_Reso        = 1000;
    LB_Motor.PWM1_UsedTIM 	  = &htim2;
	LB_Motor.PWM1_UsedChannel = TIM_CHANNEL_1;
    LB_Motor.PWM2_Reso        = 1000;
    LB_Motor.PWM2_UsedTIM 	  = &htim2;
	LB_Motor.PWM2_UsedChannel = TIM_CHANNEL_2;

    RB_Motor.PWM1_Reso        = 1000;
    RB_Motor.PWM1_UsedTIM 	  = &htim8;
	RB_Motor.PWM1_UsedChannel = TIM_CHANNEL_3;
    RB_Motor.PWM2_Reso        = 1000;
    RB_Motor.PWM2_UsedTIM 	  = &htim8;
	RB_Motor.PWM2_UsedChannel = TIM_CHANNEL_4;

	Motor_Init(&LF_Motor);
	Motor_Init(&RF_Motor);
	Motor_Init(&LB_Motor);
	Motor_Init(&RB_Motor);
}

void Chassis_SetSpeed(float vx, float vy, float omega, uint8_t isDash)
{
	// 计算每个电机的速度
	float LF_Speed =   vx + vy + omega * (WHEEL_SPACING_FB + WHEEL_SPACING_LR);
    float RF_Speed = - vx + vy - omega * (WHEEL_SPACING_FB + WHEEL_SPACING_LR);
    float LB_Speed = - vx + vy + omega * (WHEEL_SPACING_FB + WHEEL_SPACING_LR);
    float RB_Speed =   vx + vy - omega * (WHEEL_SPACING_FB + WHEEL_SPACING_LR);

	// 冲刺模式
	if(isDash > 1)isDash = 1;
    float dashPersent = isDash * 0.4f + 0.6f;

    // 计算最大速度
    float max = Chassis_max(fabsf(LF_Speed), fabsf(RF_Speed), fabsf(LB_Speed), fabsf(RB_Speed));

	// 判断电机正反转
	if(LF_Speed > 0 && LF_Motor.IsForward == 0)
	{
		Motor_Turn(&LF_Motor);
	}
	else if(LF_Speed < 0 && LF_Motor.IsForward == 1)
	{
		Motor_Turn(&LF_Motor);
	}
	if(RF_Speed > 0 && RF_Motor.IsForward == 0)
	{
		Motor_Turn(&RF_Motor);
	}
	else if(RF_Speed < 0 && RF_Motor.IsForward == 1)
	{
		Motor_Turn(&RF_Motor);
	}
	if(LB_Speed > 0 && LB_Motor.IsForward == 0)
	{
		Motor_Turn(&LB_Motor);
	}
	else if(LB_Speed < 0 && LB_Motor.IsForward == 1)
	{
		Motor_Turn(&LB_Motor);
	}
	if(RB_Speed > 0 && RB_Motor.IsForward == 0)
	{
		Motor_Turn(&RB_Motor);
	}
	else if(RB_Speed < 0 && RB_Motor.IsForward == 1)
	{
		Motor_Turn(&RB_Motor);
	}

	// 设置电机占空比
    Motor_SetDuty(fabsf(LF_Speed) / max * dashPersent, &LF_Motor);
    Motor_SetDuty(fabsf(RF_Speed) / max * dashPersent, &RF_Motor);
    Motor_SetDuty(fabsf(LB_Speed) / max * dashPersent, &LB_Motor);
    Motor_SetDuty(fabsf(RB_Speed) / max * dashPersent, &RB_Motor);
}